#include "../include/DataReader.h"
#include <boost/filesystem.hpp>
#include <fstream> // for FILE
#include <iostream>
#include <Eigen/Core>
#include <string>



namespace Estimator
{
// tool function for read one data from file
template<typename T>
int FscanfOrDie(FILE *fptr, const char *format, T* value)
{
    return  fscanf(fptr, format, value);
   
}

// tool function for substract sub string between word_left and word_right.
// for example: 
//          string input_string = "../../example.cpp";
//          string output_string = GetSubStringBetween(input_string, "/", ".cpp");
// Then we get:
//          output_string = "example"
std::string GetSubStringBetween(const std::string& input_string, const std::string& word_left, const std::string& word_right)
{
    std::size_t right_pos = input_string.rfind(word_right); 
    std::size_t left_pos = input_string.rfind(word_left) + word_left.length();
    return input_string.substr(left_pos,right_pos);                  
}

DataReader::DataReader(std::string paramFolderName)
{
    boost::filesystem::path folderpath(paramFolderName);
    if(!boost::filesystem::exists(folderpath)) {
        std::cerr << "path: " << folderpath.string() <<  " does not exist..." << std::endl;
        return;
    }

    // First, find the folder of Image
    // begin iteration for all files in this folder... 
    boost::filesystem::directory_iterator end_iter;
    for(boost::filesystem::directory_iterator file_iter(folderpath); file_iter!= end_iter; ++file_iter) {
        // check the file extension...
        if(file_iter->path().extension() == mExtension_)
            mvFileNames_.push_back(file_iter->path().string());
    }

    // sort them in increasing order..
    sortFileNames();

    // read all data from files..
    readAllDataFromFiles();

    // read camera calibration file..
    auto parampath = folderpath / "param.yaml";
    if(!boost::filesystem::exists(parampath)){
        std::cerr << "path: " << parampath.string() << " does not exist..." << std::endl;
        return;
    }
    // YamlDataReader(parampath.string());
}


void DataReader::printPoses() const
{
    for(const auto& pose : mvPoseVariables_) {
        for(size_t i = 0; i < pose.size() ; ++i)
            std::cout << pose[i] << " ";

        std::cout << std::endl;
    }

    std::cout << "Number of Poses: " << mvPoseVariables_.size() << std::endl;
}

void DataReader::printFileNames() const
{
    for(const auto& filename : mvFileNames_)
        std::cout << filename << std::endl;
}

void DataReader::printObservations() const
{
    for(const auto& obv : mMapObservationsAllFrames_)
        std::cout << obv.first.first << "," << obv.first.second << "," << obv.second[0] << "," << obv.second[1] << std::endl;

    std::cout << "Number of Observations: " << mMapObservationsAllFrames_.size() << std::endl;
}

void DataReader::printAll() const
{
    printFileNames();
    printPoses();
    printObservations();
}

void DataReader::sortFileNames()
{
    std::sort(mvFileNames_.begin(), mvFileNames_.end(), [&](const std::string& file_a,const std::string& file_b){
        // get rid of the path and extension for file_a.
        std::string substr_a = Estimator::GetSubStringBetween(file_a,"/",this->mExtension_);

        // get rid of the path and extension for file_b.
        std::string substr_b = Estimator::GetSubStringBetween(file_b,"/",this->mExtension_); 

        // return type should be bool.
        return std::stoi(substr_a) < std::stoi(substr_b);
    });
}


void DataReader::readAllDataFromFiles()
{
    // for(auto iterFileName = mvFileNames_.cbegin(); iterFileName != mvFileNames_.cend(); ++iterFileName)
    for(const auto& fileName : mvFileNames_) {
        FILE *fptr = fopen(fileName.c_str(), "r");

        if(fptr == nullptr) {
            std::cerr << "Error: unable to open file " << fileName; 
            break;
        }

        // first read the pose for this frame.
        PoseType /*std::array<double,12>*/ temPose;  
        // std::cout << "reading poses.. " << std::endl;
        std::for_each(temPose.begin(), temPose.end(), [&](double& data) {
            FscanfOrDie(fptr, "%lf", &data );
        });

        mvPoseVariables_.push_back(temPose);

        // get pose id by converting the fileName.
        int idPose;
        idPose = std::stoi(GetSubStringBetween(fileName,"/",this->mExtension_));

        // Then get landmark id and corresponding observations by scanning the file till the end.
        while(!feof(fptr)) {
            int idLandmark;
           
            FscanfOrDie(fptr, "%d", &idLandmark );

            ObservationType /*std::array<double,2>*/ temObs;
            FscanfOrDie(fptr, "%lf", &temObs[0]);
            FscanfOrDie(fptr, "%lf", &temObs[1]);

            EdgeType temEdge = std::make_pair(idPose,idLandmark);
            mMapObservationsAllFrames_[temEdge] = temObs;

            // int temUnusedData; // three zeros unused
            // FscanfOrDie(fptr, "%d", &temUnusedData);
            // FscanfOrDie(fptr, "%d", &temUnusedData);
            // FscanfOrDie(fptr, "%d", &temUnusedData);
        }

        fclose(fptr);
    }
    return;
}


std::pair<DataReader::ObservationsContainer::const_iterator, DataReader::ObservationsContainer::const_iterator>
DataReader::FindObservationRangeByFrameID(size_t idKey /*frame id*/) const
{
    EdgeType start_Key = std::make_pair(idKey, 0); // Key value for finding the start position;
    auto ite_start = mMapObservationsAllFrames_.lower_bound(start_Key);

    EdgeType end_Key = std::make_pair(idKey + 1, 0); // Key value for finding the end position;
    auto ite_end = mMapObservationsAllFrames_.upper_bound(end_Key);

    return std::make_pair(ite_start, ite_end);
}

}